﻿using System;
using System.Linq;
using System.Text;
using System.Collections.Generic;
using System.Diagnostics;

using AForge.Robotics.Lego;

namespace com.ReinforceLab.AlphaRexNxtStep
{
    class AlphaRexModel : System.ComponentModel.INotifyPropertyChanged
    {
        #region Variables
        readonly ApplicationContext context;
        NXTBrick _nxt;        
        #endregion

        #region Properties
        const string PN_IsConnected ="IsConnected";        
        public bool IsConnected { get { return _nxt.IsConnected; } }
        
        const string PN_PortName = "PortName";
        string _portName;
        public string PortName { 
            get { return _portName; } 
            set {
                //if (IsConnected || _portName == value || String.IsNullOrEmpty(_portName)) return;
                _portName = value;
                invokePropertyChanged(PN_PortName); 
            }
        }

        const string PN_FirmwareVersion = "FirmwareVersion";
        string _firmwareVersion = String.Empty;
        public string FirmwareVersion
        {
            get { return _firmwareVersion; }
            private set { _firmwareVersion = value; invokePropertyChanged(PN_FirmwareVersion); }
        }

        const string PN_ProtocolVersion = "ProtocolVersion";
        string _protocolVersion = String.Empty;
        public string ProtocolVersion
        {
            get { return _protocolVersion; }
            private set { _protocolVersion = value; invokePropertyChanged(PN_ProtocolVersion); }
        }

        const string PN_DeviceName = "DeviceName";
        string _deviceName = String.Empty;
        public string DeviceName
        {
            get { return _deviceName; }
            private set { _deviceName = value; invokePropertyChanged(PN_DeviceName); }
        }

        const string PN_BtAddress = "BtAddress";
        string _btAddress = String.Empty;
        public string BtAddress
        {
            get { return _btAddress; }
            private set { _btAddress = value; invokePropertyChanged(PN_BtAddress); }
        }

        const string PN_BtSignalStrength = "BtSignalStrength";
        string _btSignalStrength = String.Empty;
        public string BtSignalStrength
        {
            get { return _btSignalStrength; }
            private set { _btSignalStrength = value; invokePropertyChanged(PN_BtSignalStrength); }
        }

        const string PN_FreeUserFlash = "FreeUserFlash";
        string _freeUserFlash = String.Empty;
        public string FreeUserFlash
        {
            get { return _freeUserFlash; }
            private set { _freeUserFlash = value; invokePropertyChanged(PN_FreeUserFlash); }
        }

        const string PN_BatteryLevel = "BatteryLevel";
        string _batteryLevel = String.Empty;
        public string BatteryLevel
        {
            get { return _batteryLevel; }
            private set { _batteryLevel = value; invokePropertyChanged(PN_BatteryLevel); }
        }

        const string PN_Power = "Power";
        int _power = 100;
        public int Power
        {
            get { return _power; }
            set { _power = Math.Max(0, Math.Min(100, value)); invokePropertyChanged(PN_Power); }
        }
        #endregion

        #region Constructor
        public AlphaRexModel(ApplicationContext ctx)
        {
            context = ctx;
            initialize();
        }
        void initialize()
        {
            _nxt = new NXTBrick();
            PortName = "COM4";
        }
        #endregion

        #region Private methods
        void clearInformation()
        {
            this.FirmwareVersion  = String.Empty;
            this.ProtocolVersion  = String.Empty;                
            this.DeviceName       = String.Empty;
            this.BtAddress        = String.Empty;
            this.BtSignalStrength = String.Empty;
            this.FreeUserFlash    = String.Empty;
            this.BatteryLevel     = String.Empty;
        }
        void updateInformation()
        {
            string firmwareVersion;
            string protocolVersion;
            if (_nxt.GetVersion(out protocolVersion, out firmwareVersion))
            {
                this.FirmwareVersion = firmwareVersion;
                this.ProtocolVersion = protocolVersion;
            }
            else
            {
                Debug.WriteLine("Failed getting version");
            }
            // get device information
            string deviceName;
            byte[] btAddress;
            int btSinglaStrength;
            int freeUserFlash;
            if (_nxt.GetDeviceInformation(out deviceName, out btAddress, out btSinglaStrength, out freeUserFlash))
            {
                this.DeviceName = deviceName;                
                this.BtAddress = String.Format("{0}:{1}:{2}:{3}:{4}:{5}:{6}",
                    btAddress[0].ToString("X2"),
                    btAddress[1].ToString("X2"),
                    btAddress[2].ToString("X2"),
                    btAddress[3].ToString("X2"),
                    btAddress[4].ToString("X2"),
                    btAddress[5].ToString("X2"),
                    btAddress[6].ToString("X2"));
                this.BtSignalStrength = btSinglaStrength.ToString();
                this.FreeUserFlash = freeUserFlash.ToString();
            }
            else
            {
                Debug.WriteLine("Failed getting device information");
            }
            // get battery level
            int batteryLevel;            
            if (_nxt.GetBatteryPower(out batteryLevel))
            {
                this.BatteryLevel = batteryLevel.ToString();
            }
            else
            {
                Debug.WriteLine("Failed getting battery level");
            }
        }
        NXTBrick.MotorState createBaseMortorState()
        {
            NXTBrick.MotorState stat = new NXTBrick.MotorState();

            stat.Mode          = NXTBrick.MotorMode.On | NXTBrick.MotorMode.Regulated;
            stat.RunState      = NXTBrick.MotorRunState.Running;
            stat.Regulation    = NXTBrick.MotorRegulationMode.Speed;            
            stat.TachoCount    = 0;
            stat.TachoLimit    = 0;
            //stat.RotationCount = 20;

            return stat;
        }
        NXTBrick.MotorState createMortorState(int power)
        {
            var stat = createBaseMortorState();
            stat.Power = power;
            return stat;
        }
        void initFootPosition(NXTBrick.Motor mortor, NXTBrick.Sensor sensor)
        {
            _nxt.SetSensorMode(sensor, NXTBrick.SensorType.Switch, NXTBrick.SensorMode.Boolean);
            
            bool touched = false;
            while (! touched)
            {
                var stat = createMortorState(Power);
                stat.TachoLimit = 20;
                _nxt.SetMotorState(mortor, stat);
                NXTBrick.SensorValues sensorValues;
                _nxt.GetSensorValue(sensor, out sensorValues);
                touched = (sensorValues.Scaled == 1);
                System.Diagnostics.Debug.WriteLine(String.Format("\ttouched:{0}, norm:{1} raw:{2} scaled:{3}", touched, sensorValues.Normalized, sensorValues.Raw, sensorValues.Scaled));
            }
        }

        #endregion

        #region Public methods
        public void Connect()
        {
            if (IsConnected) return;

            if (String.IsNullOrEmpty(PortName))
            {
                System.Windows.MessageBox.Show("Set correct port name ", "Error", System.Windows.MessageBoxButton.OK, System.Windows.MessageBoxImage.Error);
                return;
            }
            
            if (_nxt.Connect(PortName))
            {
                context.WriteConsole("Connected successflully.");
                // invoke property changed event
                invokePropertyChanged(PN_IsConnected);
                // get firmware/protocol version
                updateInformation();
            }
            else
            {
                System.Windows.MessageBox.Show("Failed connecting ToString NXT device", "Error", System.Windows.MessageBoxButton.OK, System.Windows.MessageBoxImage.Error);
            }
        }
        public void Disconnect()
        {
            if (!IsConnected) return;
            
            _nxt.Disconnect();
            clearInformation();
        }        
        public void Step(int left, int right)
        {
            if (!IsConnected) return;
            var stat = createMortorState(left);
            _nxt.SetMotorState(NXTBrick.Motor.B, stat);
            
            stat = createMortorState(left);
            _nxt.SetMotorState(NXTBrick.Motor.C, stat);

            // arm
            int tot = Math.Abs(left) + Math.Abs(right);
            stat = createMortorState(tot / 3);
            _nxt.SetMotorState(NXTBrick.Motor.A, stat);

        }
/*
        public void InitFootPosition()
        {
            initFootPosition(NXTBrick.Motor.B, NXTBrick.Sensor.First);
            initFootPosition(NXTBrick.Motor.C, NXTBrick.Sensor.Second);

            var stat = createMortorState(Power);
            stat.TachoLimit = 180;
            _nxt.SetMotorState(NXTBrick.Motor.B, stat);
        }
        public void GoForward()
        {
            if (! IsConnected) return;

            var stat = createMortorState(Power);
            _nxt.SetMotorState(NXTBrick.Motor.B, stat);
            _nxt.SetMotorState(NXTBrick.Motor.C, stat);
        }
        public void GoBackward()
        {
            if (!IsConnected) return;

            var stat = createMortorState(Power * -1);            
            _nxt.SetMotorState(NXTBrick.Motor.B, stat);
            _nxt.SetMotorState(NXTBrick.Motor.C, stat);
        }
        public void TurnLeft()
        {
            if (!IsConnected) return;

            var stat = createMortorState(Power);
            _nxt.SetMotorState(NXTBrick.Motor.B, stat);
        }
        public void TurnRight()
        {
            if (!IsConnected) return;

            var stat = createMortorState(Power * -1);            
            _nxt.SetMotorState(NXTBrick.Motor.C, stat);
        }*/
        #endregion

        #region INotifyPropertyChanged メンバ
        public event System.ComponentModel.PropertyChangedEventHandler PropertyChanged;
        void invokePropertyChanged(String name)
        {
            if (null != PropertyChanged)
                PropertyChanged.Invoke(this, new System.ComponentModel.PropertyChangedEventArgs(name));
        }
        #endregion
    }
}
